/**
* Selection tools. So we can click on faces and verticies
* to select and deselect them.  Use the "selection" shader
* to see what is going on here.
*
* Clicking a vertex/face will toggle its selection status.
*/

#include <OGF/surface/tools/surface_facet_tools.h>
#include <OGF/quick_start/tools/surface_quick_start_tool.h>
#include <OGF/surface/tools/surface_picker.h>
#include <OGF/cells/map/map_attributes.h>

#include <OGF/quick_start/common/select_util.h>
#include <OGF/cells/map/map_cell_heap.h>

//#include <OGF/quick_start/common/simplifier_math.h>
#include <time.h>
#include <sstream>

// see OGF/surface/tools/surface_facet_tools.cpp for examples

namespace OGF {
    

	////////////////////////////////////////
	/// PAINT STIFFNESS   //////////////////
	////////////////////////////////////////
/*
	StiffnessPaintTool::StiffnessPaintTool(
        	ToolsManager* manager
    	) : SurfaceTool(manager) 
	{
		//do nothing
    	}
*/
	/*
	 * Paint a vertex as stiff
	 * Set a stiffness attribute
	 */
/*	void StiffnessPaintTool::grab(const RayPick& p_ndc)
	{
		Map::Vertex* picked = picker()->pick_vertex(p_ndc);
		if(picked == nil){
			return;
		}

		MapVertexAttribute<double> stiffness(surface(), "stiffness");
		// If the vertex is already stiff, turn down stiffness
		// Otherwise set it to some arbitrary high stiffness
		if(stiffness[picked] == 1.0){
			stiffness[picked] = 0.0;
		} else {
			stiffness[picked] = 1.0;
		}

		surface()->update();

	}

    	void StiffnessPaintTool::reset() 
	{
        	SurfaceTool::reset() ;
	}  */


	////////////////////////////////////////
	/// PAINT BORDER ///////////////////////
	////////////////////////////////////////

    BorderPaintTool::BorderPaintTool(
        ToolsManager* manager
    ) : SurfaceTool(manager) 
	{
		//do nothing
    }
	void BorderPaintTool::grab(const RayPick& p_ndc)
	{
		this->origin = nil;
		origin = picker()->pick_facet(p_ndc);
	}
	/**
	* Paint a path of border verticies between the last
	* drag pick and this one. Alternatively, remove border verts.
	*/
	void BorderPaintTool::drag(const RayPick& p_ndc)
	{
		//std::cout<<"Button = "<<p_ndc.button<<std::endl;
		Map::Facet* f = picker()->pick_facet(p_ndc);
        if(f != nil) 
		{
			deform.lazy_init(this->surface());
			if(p_ndc.button != 3) //any button other than right mouse button
				deform.add_to_border(f);
			else
				deform.remove_from_border(f);

			if(origin != nil)
				this->connect_facets(origin, f, p_ndc.button != 3);
			origin = f;

			deform.grow_region();
			surface()->update();
        }
    }
    void BorderPaintTool::reset() 
	{
        SurfaceTool::reset() ;
    }  
	/**
	* Get some measure of the center of a face.
	*/
	Point3d BorderPaintTool::get_face_center(Map::Facet *face)
	{
		double x,y,z, n;
		x=y=z=n=0;

		Map::Vertex *v;
		FOR_EACH_VERTEX_IN_FACE(face, v)
		{
			Point3d p = v->point();
			z += p.x();
			z += p.y();
			z += p.z();
			n += 1;
		}
		return Point3d(x/n, y/n, z/n);
	}
	/**
	* Find a path between 'from' and 'to' and add/remove
	* all of those faces from the boundary.
	* This code is lifted from elsewhere in graphite. I think
	* it is doing A* search.
	*/
	void BorderPaintTool::connect_facets(Map::Facet *from, Map::Facet *to, bool add)
	{		
        MapFacetAttribute<double> distance(surface()) ;
        MapFacetAttribute<double> distance_estimate(surface()) ;
        MapFacetAttribute<Map::Facet*> previous_node(surface()) ;
        
;
        {FOR_EACH_FACET(Map, surface(), it) {
            distance[it] = 1e19;
            distance_estimate[it] = 1e19;
            previous_node[it] = nil ;
        }}

        MapFacetHeap Q(surface(), distance_estimate) ;
        Q.init_with_all_surface_facets();        

        bool target_hit = false ;
        
        distance[from] = 0;
		distance_estimate[from] = ( get_face_center(from) - get_face_center(to) ).norm() ;
    
        Q.update_cost(from);

        while (!target_hit && !Q.empty()) {
            Map::Facet* u = Q.pop() ;
            Map::Halfedge* h = u->halfedge();
            do {
				Map::Facet* v = h->opposite()->facet();
				double dist = distance[u] + (get_face_center(u) - get_face_center(v)).norm();
                if (distance[v] > dist) 
				{
                    distance[v] = dist;
                    distance_estimate[v] = distance[v] + ( get_face_center(v) - get_face_center(to) ).norm();
                    previous_node[v] = u;
                    if(Q.contains(v)) {
                        Q.update_cost(v) ;
                    }
                    if(v == to) {
                        target_hit = true ;
                    }
                }
                h=h->next() ;
            } while (h != u->halfedge());
        }

        if(!target_hit) {
            return ;	//if we couldn't make the path...
        }

        Map::Facet* v = to ;
        do {
			if(add)
				deform.add_to_border(v);
			else
				deform.remove_from_border(v);
            v = previous_node[v] ;
        } while (v!=from) ;
			if(add)
				deform.add_to_border(v);
			else
				deform.remove_from_border(v);
	}


	////////////////////////////////////////
	/// ANCHOR   ///////////////////////////
	////////////////////////////////////////

    DeformAnchorTool::DeformAnchorTool(
        ToolsManager* manager
    ) : SurfaceTool(manager) 
	{
		picked_vertex_ = nil;
    }

	/**
	* Pick the anchor vertex.
	*/
    void DeformAnchorTool::grab(const RayPick& p_ndc)
	{
		//Grab a vertex and set it as anchor.
		//Do nothing if we didn't click a vertex

		deform.lazy_init(surface());

		picked_vertex_ = picker()->pick_vertex(p_ndc);

		if(picked_vertex_ == nil)
			return;

		deform.set_anchor(picked_vertex_);	
		true_norm = deform.get_true_anchor_normal();

		surface()->update();
	}

	/**
	* Drag the anchor around, or rotate its normal.
	*/
    void DeformAnchorTool::drag(const RayPick& p_ndc)
	{
		static bool is_deforming = false;

		//If we are doing nothing...
		if(picked_vertex_ == nil || deform.get_anchor() == nil)
			return;
		
		//move anchor
		Point3d new_pos = picker()->drag_point3d(p_ndc);
		picked_vertex_->set_point(new_pos);

		if(!is_deforming)
		{
			//potential for a race condition here. But it's ok if that happens
			is_deforming = true;
			deform.do_fast_deformation();
			is_deforming = false;
			surface()->update() ;
		}

	}
    void DeformAnchorTool::release(const RayPick& p_ndc)
	{
		if(picked_vertex_ != nil && deform.get_anchor() != nil)
		{
			deform.do_deformation();
			surface()->update();
		}

        picked_vertex_ = nil;
	}

    void DeformAnchorTool::reset() 
	{
        SurfaceTool::reset() ;
		picked_vertex_ = nil;
    }
}
